////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "BoundingVolumes.h"

#include <SFML/OpenGL.hpp>

#include "../Math.hpp"

#ifdef _DEBUG
#include <SFML/Graphics.hpp>
#endif

#include <SmegTools/Debug/DebugNew.hpp>

namespace smeg {
    namespace math {
        
////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
BoundingBox::BoundingBox( const sf::Vector2f& _Center, const sf::Vector2f& _Size ,float _fRotation ) :
    BoundingVolume( eBoundingBox, _Center, _Size ),
    m_bIsAxisAligned( true ),
    m_fRotation( _fRotation * math::DEG_TO_RAD )
{
    m_Axis[0] = sf::Vector2f( 1.0f, 0.0f );
    m_Axis[1] = sf::Vector2f( 0.0f, 1.0f );
    ComputeBox();
}
        
////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
BoundingBox::BoundingBox( const BoundingBox& _Copy ) :
    BoundingVolume( _Copy ),
    m_bIsAxisAligned( _Copy.m_bIsAxisAligned ),
    m_fRotation( _Copy.m_fRotation )
{
    for ( int i = 0; i < 4; ++i ) {
        m_Corners[i] = _Copy.m_Corners[i];
    }
    for ( int i = 0; i < 2; ++i ) {
        m_Axis[i] = _Copy.m_Axis[i];
    }
}

////////////////////////////////////////////////////////////
/// Destructor
////////////////////////////////////////////////////////////
BoundingBox::~BoundingBox()
{
}

////////////////////////////////////////////////////////////
/// Setup the BoundingBox
////////////////////////////////////////////////////////////
void BoundingBox::Setup( const sf::Vector2f& _Center, const sf::Vector2f& _Size, float _fRotation ) {
    BoundingVolume::Setup( _Center, _Size );
    SetRotation( _fRotation );
}

////////////////////////////////////////////////////////////
/// Does the Bounding Volume overlaps with another volume
////////////////////////////////////////////////////////////
bool BoundingBox::Overlaps( const BoundingBox& _Other ) const {
    if ( m_bIsAxisAligned && _Other.m_bIsAxisAligned ) {
        return !( m_Corners[eBottomRight].x < _Other.m_Corners[eTopLeft].x ||
            m_Corners[eBottomRight].y < _Other.m_Corners[eTopLeft].y ||
            m_Corners[eTopLeft].x > _Other.m_Corners[eBottomRight].x ||
            m_Corners[eTopLeft].y > _Other.m_Corners[eBottomRight].y );
    } else {
        // Translation in box's frame
        sf::Vector2f VecToOtherCenter = math::PointToLocalSpace(    _Other.m_Center,
                                                                    m_Axis[eXAxis],
                                                                    m_Axis[eYAxis],
                                                                    m_Center );

        // Compute rotation matrix from this to _Other
        float RotMat[2][2],
            fRa, fRb, fT,
            A[2] = { m_HalfSize.x, m_HalfSize.y },
            B[2] = { _Other.m_HalfSize.x, _Other.m_HalfSize.y },
            Vec[2] = { abs( VecToOtherCenter.x ), abs( VecToOtherCenter.y ) };

        for ( int i = 0; i < 2; ++i ) {
            for ( int j = 0; j < 2; ++j ) {
                RotMat[i][j] = math::Dot( m_Axis[i], _Other.m_Axis[j] );
            }
        }

        // Overlap in this box dimension
        for ( int i = 0; i < 2; ++i ) {
            fRa = A[i];
            fRb = B[0] * abs( RotMat[0][i] ) + B[1] * abs( RotMat[1][i] );

            if ( Vec[i] > fRa + fRb ) {
                return false;
            }
        }

        // Overlap in _Other box dimension
        for ( int i = 0; i < 2; ++i ) {
            fRa = A[0] * abs( RotMat[i][0] ) + A[1] * abs( RotMat[i][1] );
            fRb = B[i];

            fT = abs( Vec[0] * RotMat[i][0] + Vec[1] * RotMat[i][1] );

            if ( fT > fRa + fRb ) {
                return false;
            }
        }

        return true;
    }
}

bool BoundingBox::Overlaps( const Line& _Other ) const {
    return BoundingVolume::Overlaps( *this, _Other );
}

bool BoundingBox::Overlaps( const Circle& _Other ) const {
    return BoundingVolume::Overlaps( *this, _Other );
}

////////////////////////////////////////////////////////////
/// Does the Bounding Volume contains another Volume
////////////////////////////////////////////////////////////
bool BoundingBox::Contains( const BoundingBox& _Other ) const {
    if ( m_bIsAxisAligned && _Other.m_bIsAxisAligned ) {
        return ( _Other.m_Corners[eTopLeft].x >= m_Corners[eTopLeft].x &&
            _Other.m_Corners[eTopLeft].y >= m_Corners[eTopLeft].y &&
            _Other.m_Corners[eBottomRight].x <= m_Corners[eBottomRight].x &&
            _Other.m_Corners[eBottomRight].y <= m_Corners[eBottomRight].y );
    } else {
        return Contains( _Other.m_Corners[eTopLeft] ) &&
            Contains( _Other.m_Corners[eTopRight] ) &&
            Contains( _Other.m_Corners[eBottomLeft] ) &&
            Contains( _Other.m_Corners[eBottomRight] ); 
    }
}

bool BoundingBox::Contains( const Line& _Other ) const {
    const sf::Vector2f* _pLineExtents = _Other.GetExtents();

    return ComputeOutCode( _pLineExtents[0].x, _pLineExtents[0].y ) == INSIDE &&
        ComputeOutCode( _pLineExtents[1].x, _pLineExtents[1].y ) == INSIDE;
}

bool BoundingBox::Contains( const Circle& _Other ) const {
    sf::Vector2f circleCenter = _Other.GetCenter() - m_Center;
    if ( !m_bIsAxisAligned ) {
        circleCenter = math::VectorToLocalSpace(    circleCenter,
                                                    m_Axis[eXAxis],
                                                    m_Axis[eYAxis] );
    }

    float circleRadius = _Other.GetRadius();

    return ( circleCenter.x - circleRadius >= -m_HalfSize.x &&
        circleCenter.y - circleRadius >= -m_HalfSize.y &&
        circleCenter.x + circleRadius <= m_HalfSize.x &&
        circleCenter.y + circleRadius <= m_HalfSize.y );
}

////////////////////////////////////////////////////////////
/// Does the Bounding Volume contains a point
////////////////////////////////////////////////////////////
bool BoundingBox::Contains( const sf::Vector2f& _Point ) const {
    return ComputeOutCode( _Point.x, _Point.y ) == INSIDE;
}

////////////////////////////////////////////////////////////
/// Helper function for the Cohen-Sutherland algorithm
/// http://en.wikipedia.org/wiki/Cohen%E2%80%93Sutherland_algorithm
////////////////////////////////////////////////////////////
int BoundingBox::ComputeOutCode( float _fX, float _fY ) const {
    int iCode = INSIDE;

    if ( m_bIsAxisAligned ) {
        if ( _fX < m_Corners[eTopLeft].x ) {
            iCode |= LEFT;
        } else if ( _fX > m_Corners[eBottomRight].x ) {
            iCode |= RIGHT;
        }
    
        if ( _fY < m_Corners[eTopLeft].y ) {
            iCode |= TOP;
        } else if ( _fY > m_Corners[eBottomRight].y ) {
            iCode |= BOTTOM;
        }
    } else {
        sf::Vector2f PointToLocal( _fX, _fY );

        PointToLocal = math::PointToLocalSpace( PointToLocal,
                                                m_Axis[eXAxis],
                                                m_Axis[eYAxis],
                                                m_Center );

        if ( PointToLocal.x < -m_HalfSize.x ) {
            iCode |= LEFT;
        } else if ( PointToLocal.x > m_HalfSize.x ) {
            iCode |= RIGHT;
        }
    
        if ( PointToLocal.y < -m_HalfSize.y ) {
            iCode |= TOP;
        } else if ( PointToLocal.y > m_HalfSize.y ) {
            iCode |= BOTTOM;
        }
    }

    return iCode;
}

////////////////////////////////////////////////////////////
/// Compute the bounds of the BoundingBox
////////////////////////////////////////////////////////////
void BoundingBox::ComputeBox() {
    if ( math::IsZeroByEpsilon( m_fRotation ) ) {
        m_bIsAxisAligned = true;
        
        m_Corners[eTopLeft] = m_Center - m_HalfSize;
        m_Corners[eTopRight] = m_Center + sf::Vector2f( m_HalfSize.x, -m_HalfSize.y );
        m_Corners[eBottomRight] = m_Center + m_HalfSize;
        m_Corners[eBottomLeft] = m_Center + sf::Vector2f( -m_HalfSize.x, m_HalfSize.y );
    } else {
        m_bIsAxisAligned = false;
        float fCos = cos( m_fRotation ),
            fSin = sin( m_fRotation );
        m_Axis[eXAxis] = sf::Vector2f( fCos, fSin );
        m_Axis[eYAxis] = sf::Vector2f( -fSin, fCos );

        m_Corners[eTopLeft] = m_Center - m_Axis[eXAxis] * m_HalfSize.x - m_Axis[eYAxis] * m_HalfSize.y;
        m_Corners[eTopRight] = m_Center + m_Axis[eXAxis] * m_HalfSize.x - m_Axis[eYAxis] * m_HalfSize.y;
        m_Corners[eBottomRight] = m_Center + m_Axis[eXAxis] * m_HalfSize.x + m_Axis[eYAxis] * m_HalfSize.y;
        m_Corners[eBottomLeft] = m_Center - m_Axis[eXAxis] * m_HalfSize.x + m_Axis[eYAxis] * m_HalfSize.y;
    }
}

/*********************** Accessors ************************/
void BoundingBox::SetCenter( float _fX, float _fY ) {
    BoundingVolume::SetCenter( _fX, _fY );
    ComputeBox();
}

void BoundingBox::MoveCenter( float _fX, float _fY ) {
    BoundingVolume::MoveCenter( _fX, _fY );
    ComputeBox();
}

void BoundingBox::SetRotation( float _fRotation ) {
    m_fRotation = _fRotation * math::DEG_TO_RAD;
    ComputeBox();
}

Line BoundingBox::GetLeftLine() const {
    Line AABBLine;
    AABBLine.Setup( m_Corners[eTopLeft], m_Corners[eBottomLeft] );

    return AABBLine;
}

Line BoundingBox::GetRightLine() const {
    Line AABBLine;
    AABBLine.Setup( m_Corners[eTopRight], m_Corners[eBottomRight] );

    return AABBLine;
}

Line BoundingBox::GetTopLine() const {
    Line AABBLine;
    AABBLine.Setup( m_Corners[eTopLeft], m_Corners[eTopRight] );

    return AABBLine;
}

Line BoundingBox::GetBottomLine() const {
    Line AABBLine;
    AABBLine.Setup( m_Corners[eBottomRight], m_Corners[eBottomLeft] );

    return AABBLine;
}

float BoundingBox::GetPseudoRadius() const {
    return sqrt( m_HalfSize.x * m_HalfSize.x + m_HalfSize.y * m_HalfSize.y );
}

#ifdef _DEBUG
void BoundingBox::DebugRender( sf::RenderWindow* _pRenderWindow, const sf::Color& _Color, bool _bFull ) const {
    sf::RectangleShape Rect( m_Size );
    Rect.setOrigin( m_HalfSize );
    Rect.setPosition( m_Center );
    Rect.setRotation( m_fRotation * math::RAD_TO_DEG );

    if ( _bFull ) {
        Rect.setFillColor( _Color );
    } else {
        Rect.setFillColor( sf::Color::Transparent );
        Rect.setOutlineColor( _Color );
        Rect.setOutlineThickness( 1.0f );
    }

    _pRenderWindow->draw( Rect );
}
#endif // _DEBUG

    } // namespace math
} // namespace smeg